/*
 * ContextLearnerUtils.h
 *
 *  Created on: May 31, 2011
 *      Author: alper
 */

#ifndef ContextLearnerUtils_H_
#define ContextLearnerUtils_H_

#include <math.h>
//OpenCV
#include "cv.h"
#include "highgui.h"

namespace findobject{

    //TODO: Add descriptions for each feature
enum FEATURETYPES
    {
      NORMALSUM_FEATURE = 1<<1,
      POSSUM_FEATURE = 1<<2,
      COLOURSUM_FEATURE = 1<<3
    };

/*
 *  This Quickselect routine is based on the algorithm described in
 *  "Numerical recipes in C", Second Edition,
 *  Cambridge University Press, 1992, Section 8.5, ISBN 0-521-43108-5
 *  This code by Nicolas Devillard - 1998. Public domain.
 */

/*
 Checks if a point is valid to use within calculations.
 Used to make sure its not a point with bad data given from the kinect.
 */
inline bool is_valid(Point* p){
        return  !(p->z==0) && !isnan(p->x)&& !isnan(p->y)&& !isnan(p->z) && !((p->norm_x==0)&&(p->norm_y==0)&&(p->norm_z==0))
            && !isnan(p->norm_x) && !isnan(p->norm_y) && !isnan(p->norm_z);
}

/*
 Checks if a point is valid to use within calculations and is part of an object.
 Used to make sure its not a point with bad data given from the kinect.
 */
inline bool is_valid_and_object(Point* p){
        return (p->is_object) && !(p->z==0) && !isnan(p->x)&& !isnan(p->y)&& !isnan(p->z)&&!(p->norm_x==0)&&!(p->norm_y==0)&&!(p->norm_z==0);//;
}

/*
 Checks if a point is valid to use within calculations and makes sure it is not a part of an object.
 Used to make sure its not a point with bad data given from the kinect.
 */
inline bool is_valid_and_not_object(Point* p){
        return !(p->z==0) && !isnan(p->x)&& !isnan(p->y)&& !isnan(p->z) &&!((p->norm_x==0)&&(p->norm_y==0)&&(p->norm_z==0));//!(p->is_object) && !(p->z==0) && !isnan(p->x)&& !isnan(p->y)&& !isnan(p->z)&&!(p->norm_x==0)&&!(p->norm_y==0)&&!(p->norm_z==0);//;
}


#define ELEM_SWAP(a,b) { register float t=(a);(a)=(b);(b)=t; }

inline float quick_select(float * arr, int n)
{
    int low, high ;
    int median;
    int middle, ll, hh;

    low = 0 ; high = n-1 ; median = (low + high) / 2;
    for (;;) {
        if (high <= low)
            return arr[median] ;

        if (high == low + 1) {
            if (arr[low] > arr[high])
                ELEM_SWAP(arr[low], arr[high]) ;
            return arr[median] ;
        }


    middle = (low + high) / 2;
    if (arr[middle] > arr[high])    ELEM_SWAP(arr[middle], arr[high]) ;
    if (arr[low] > arr[high])       ELEM_SWAP(arr[low], arr[high]) ;
    if (arr[middle] > arr[low])     ELEM_SWAP(arr[middle], arr[low]) ;


    ELEM_SWAP(arr[middle], arr[low+1]) ;


    ll = low + 1;
    hh = high;
    for (;;) {
        do ll++; while (arr[low] > arr[ll]) ;
        do hh--; while (arr[hh]  > arr[low]) ;

        if (hh < ll)
        break;

        ELEM_SWAP(arr[ll], arr[hh]) ;
    }


    ELEM_SWAP(arr[low], arr[hh]) ;


    if (hh <= median)
        low = ll;
        if (hh >= median)
        high = hh - 1;
    }
}

#undef ELEM_SWAP

};

#endif

